import lejos.nxt.*;
import lejos.nxt.comm.*;
import java.io.*;
import lejos.robotics.navigation.*;

public class NXTLocalization 
{
	static final int FORWARD = 1;
	static final int BACKWARD = -1;
	static final int STOP = 0;
	
	public static void main(String[] args) throws Exception 
	{
		
		int command = STOP;
		
		UltrasonicSensor sonic = new UltrasonicSensor(SensorPort.S1);
		TachoPilot robot = new TachoPilot(2.17f, 2.17f,1.38f,Motor.A, Motor.C, false);
		robot.setSpeed(50);
		
		String connected = "Connected...";
		String waiting = "Waiting...";

		LCD.drawString(waiting,0,0);
		LCD.refresh();

		BTConnection btc = Bluetooth.waitForConnection();
        
		LCD.clear();
		LCD.drawString(connected,0,0);
		LCD.refresh();	
		
		DataInputStream dis = btc.openDataInputStream();
		DataOutputStream dos = btc.openDataOutputStream();
		
		while(!Button.ESCAPE.isPressed()) 
		{
			LCD.clear();
			LCD.drawString(sonic.getVersion(), 0, 0);
			LCD.drawString(sonic.getProductID(), 0, 1);
			LCD.drawString(sonic.getSensorType(), 0, 2);
			Thread.sleep(200);
			LCD.drawInt(sonic.getDistance(), 0, 3);
			
			
			command = dis.readInt();
			
			if (command == FORWARD) {
				robot.forward();
				LCD.drawString("forward...", 0, 5);
			}
			else if (command == BACKWARD) {
				robot.backward();
				LCD.drawString("backward...", 0, 5);
			}
			else if (command == STOP) {
				robot.stop();
				LCD.drawString("stop....", 0, 5);
			}
			
			LCD.refresh();
			Thread.sleep(500);
			
			dos.writeInt(Math.round(robot.getTravelDistance()));
			dos.flush();
			robot.reset();

			dos.writeInt(sonic.getDistance());
			dos.flush();
			
		}
		
		btc.close();
		LCD.clear();
	}

}

